#include "zf_common_headfile.h"

motor_param motor_l;
motor_param motor_r;

pid_param_t motor_pid_l;
pid_param_t motor_pid_r;
pid_param_t servo_pid;

// 小车移动标志位
int protect_flag = 0;

float servo_duty;

// 一般预瞄点
float aim_distance_flash;
// 弯道预瞄点
float bend_check_aim_distance, bend_aim_distance_flash;
// 圆环预瞄点
float circle_aim_distance_flash;
// 十字预瞄点
float cross_aim_distance_flash;

// 速度
int set_speed;
// 一般速度
int set_speed_flash;
// 弯道速度
int bend_set_speed_flash;
// 圆环速度
int circle_set_speed_flash;
// 十字速度
int cross_set_speed_flash;
// 坡道速度
int ramp_set_speed_flash;

IfxCpu_mutexLock g_MutexUart;

void reset_all (void)
{
    ips200_full(RGB565_RED);

    cross_type = CROSS_NONE;
    circle_type = CIRCLE_NONE;
    ramp_type = RAMP_NONE;
    stop_car_type = STOP_NONE;

    servo_pid.out_p = servo_pid.out_i = servo_pid.out_d = 0;

    motor_l.total_encoder = motor_l.current_speed = 0;
    motor_r.total_encoder = motor_r.current_speed = 0;

    motor_pid_l.error = motor_pid_l.pre_error = motor_pid_l.pre_pre_error = 0;
    motor_pid_l.out_p = motor_pid_l.out_i = motor_pid_l.out_d = 0;
    motor_pid_r.error = motor_pid_r.pre_error = motor_pid_r.pre_pre_error = 0;
    motor_pid_l.out_p = motor_pid_l.out_i = motor_pid_l.out_d = 0;

    ips200_full(RGB565_BLUE);
    system_delay_ms(300);
    ips200_full(RGB565_WHITE);
    system_delay_ms(200);

    set_speed = (set_speed_flash + bend_set_speed_flash + circle_set_speed_flash + cross_set_speed_flash) / 4;
    aim_distance = (aim_distance_flash + bend_aim_distance_flash + circle_aim_distance_flash + cross_aim_distance_flash) / 4;

    protect_flag = 1;
}
